/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include <stdbool.h>

#define M_PI_F (float)3.14159265
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_Q 1.0f / sqrtf(2.0f)

struct filter_s;
typedef struct filter_s filter_t;
typedef struct pt1Filter_s
{
    float state;
    float k;
} pt1Filter_t;
// 定义滤波器结构体
typedef struct
{
    float b0, b1, b2;
    float a1, a2;
    float delay_element_1;
    float delay_element_2;
} LPFData;
/* this holds the data required to update samples thru a filter */
typedef struct biquadFilter_s
{
    float b0, b1, b2, a1, a2;
    float x1, x2, y1, y2;
} biquadFilter_t;
typedef struct laggedMovingAverage_s
{
    uint16_t movingWindowIndex;
    uint16_t windowSize;
    float movingSum;
    float *buf;
    bool primed;
} laggedMovingAverage_t;

typedef enum
{
    FILTER_PT1 = 0,
    FILTER_BIQUAD,
} lowpassFilterType_e;

typedef enum
{
    FILTER_LPF, // 2nd order Butterworth section
    FILTER_NOTCH,
    FILTER_BPF,
} biquadFilterType_e;
float pt1FilterGain(float f_cut, float dT)
{
    float RC = 1 / (2 * M_PI_FLOAT * f_cut);
    return dT / (RC + dT);
}

void pt1FilterInit(pt1Filter_t *filter, float k)
{
    filter->state = 0.0f;
    filter->k = k;
}

void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
{
    filter->k = k;
}

float pt1FilterApply(pt1Filter_t *filter, float input)
{
    filter->state = filter->state + filter->k * (input - filter->state);
    return filter->state;
}
typedef float (*filterApplyFnPtr)(filter_t *filter, float input);

float nullFilterApply(filter_t *filter, float input);

void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);

float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);

void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);

float pt1FilterGain(float f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);

// get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
// Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
float filterGetNotchQ(float centerFreq, float cutoffFreq)
{
    return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
}
void arm_radix8_butterfly_f32(
    float *pSrc,
    uint16_t fftLen,
    const float *pCoef,
    uint16_t twidCoefModifier);
void arm_mult_f32(
    float *pSrcA,
    float *pSrcB,
    float *pDst,
    uint32_t blockSize);
/* sets up a biquad Filter */
/**
 * @brief
 *
 * @param filter
 * @param filterFreq cutoff freq (Hz)
 * @param refreshRate refresh time (us)
 */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
    biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
/**
 * @brief
 *
 * @param filter
 * @param filterFreq Hz
 * @param refreshRate time in us
 * @param Q
 * @param filterType
 */
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
    // setup variables
    const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
    // const float omega = 2.0f * M_PI_FLOAT * filterFreq / refreshRate;
    const float sn = sin(omega);
    const float cs = cos(omega);
    const float alpha = sn / (2.0f * Q);

    float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;

    switch (filterType)
    {
    case FILTER_LPF:
        // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
        // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
        b0 = (1 - cs) * 0.5f;
        b1 = 1 - cs;
        b2 = (1 - cs) * 0.5f;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case FILTER_NOTCH:
        b0 = 1;
        b1 = -2 * cs;
        b2 = 1;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case FILTER_BPF:
        b0 = alpha;
        b1 = 0;
        b2 = -alpha;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    }

    // precompute the coefficients
    filter->b0 = b0 / a0;
    filter->b1 = b1 / a0;
    filter->b2 = b2 / a0;
    filter->a1 = a1 / a0;
    filter->a2 = a2 / a0;

    // zero initial samples
    filter->x1 = filter->x2 = 0;
    filter->y1 = filter->y2 = 0;
}

void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
    // backup state
    float x1 = filter->x1;
    float x2 = filter->x2;
    float y1 = filter->y1;
    float y2 = filter->y2;

    biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);

    // restore state
    filter->x1 = x1;
    filter->x2 = x2;
    filter->y1 = y1;
    filter->y2 = y2;
}

void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
    biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}

/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
{
    /* compute result */
    const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;

    /* shift x1 to x2, input to x1 */
    filter->x2 = filter->x1;
    filter->x1 = input;

    /* shift y1 to y2, result to y1 */
    filter->y2 = filter->y1;
    filter->y1 = result;

    return result;
}

/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApply(biquadFilter_t *filter, float input)
{
    const float result = filter->b0 * input + filter->x1;
    filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
    filter->x2 = filter->b2 * input - filter->a2 * result;
    return result;
}

// void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
// float slewFilterApply(slewFilter_t *filter, float input);

// float KalmanFilter(Kalman_t *kfp,float input);
// void KalmanFilterInit(Kalman_t *kfp);
// void KalmanFilterInitParam(Kalman_t *kfp, float q, float r);
// void lpf2pInit(lpf2pData* lpfData, float sample_freq, float cutoff_freq);
// void lpf2pSetCutoffFreq(lpf2pData *lpfData, float sample_freq, float cutoff_freq);
// float lpf2pApply(lpf2pData* lpfData, float sample);
// 初始化滤波器系数
void init_lpf(LPFData *lpfData, float sample_freq, float cutoff_freq)
{
    if (cutoff_freq <= 0.0f)
    {
        printf("Error: cutoff frequency must be greater than 0.\n");
        return;
    }

    // const float M_PI_F = 3.14159265358979323846f;
    float fr = sample_freq / cutoff_freq;
    float ohm = tanf(M_PI_F / fr);
    float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;

    lpfData->b0 = ohm * ohm / c;
    lpfData->b1 = 2.0f * lpfData->b0;
    lpfData->b2 = lpfData->b0;
    lpfData->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
    lpfData->a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
    lpfData->delay_element_1 = 0.0f;
    lpfData->delay_element_2 = 0.0f;
}

// 滤波函数
float filter(LPFData *lpfData, float input)
{
    float output = lpfData->b0 * input + lpfData->b1 * lpfData->delay_element_1 + lpfData->b2 * lpfData->delay_element_2 - lpfData->a1 * lpfData->delay_element_1 - lpfData->a2 * lpfData->delay_element_2;

    lpfData->delay_element_2 = lpfData->delay_element_1;
    lpfData->delay_element_1 = input;

    return output;
}

// 定义二阶IIR滤波器结构体
typedef struct
{
    float a0, a1, a2; // 输入信号系数
    float b1, b2;     // 输出信号系数
    float x1, x2;     // 前两个输入信号
    float y1, y2;     // 前两个输出信号
} SecondOrderIIRFilter;

#define SAMPLE_RATE 1000.0f
#define CUTOFF_FREQ 100
#define SIGNAL_LENGTH 128

#define HEIGHT 600
#define WIDTH SIGNAL_LENGTH
#define notch_CENTER_FREQ 300
#define notch_cutoff 50
#define FFT_WINDOW_SIZE SIGNAL_LENGTH
float orign[SIGNAL_LENGTH];
float input_signal[SIGNAL_LENGTH];
float output_signal1[SIGNAL_LENGTH];
float output_signal2[SIGNAL_LENGTH];

int generate_ware()
{
    LPFData lpf;
    biquadFilter_t lpf_filter;
    biquadFilter_t notch_filter;
    pt1Filter_t pt1_filter;
    float looptime = 1000000 / SAMPLE_RATE;

    biquadFilterInitLPF(&lpf_filter, CUTOFF_FREQ, looptime);
    init_lpf(&lpf, SAMPLE_RATE, CUTOFF_FREQ);

    float notchQ = filterGetNotchQ(notch_CENTER_FREQ, notch_cutoff);
    biquadFilterInit(&notch_filter, notch_CENTER_FREQ, looptime, notchQ, FILTER_NOTCH);
    pt1FilterInit(&pt1_filter, pt1FilterGain(CUTOFF_FREQ, 0.001f));

    // 生成测试信号
    // 生成测试信号（包含 50Hz 和 200Hz 的正弦波）
    // const float M_PI_F = 3.14159265358979323846f;
    for (int i = 0; i < SIGNAL_LENGTH; i++)
    {
        float t = (float)i / SAMPLE_RATE;
        orign[i] = sinf(2 * M_PI_F * 50 * t);
        input_signal[i] = orign[i] + sinf(2 * M_PI_F * 200 * t) + sinf(2 * M_PI_F * 300 * t);
    }

    // 进行滤波
    for (int i = 0; i < SIGNAL_LENGTH; i++)
    {
        output_signal1[i] = biquadFilterApply(&notch_filter, input_signal[i]);
        output_signal2[i] = biquadFilterApply(&lpf_filter, output_signal1[i]);
        // output_signal2[i] = pt1FilterApply(&pt1_filter, output_signal1[i]);
    }

    // 将输入和输出信号保存到文件
    FILE *fp = fopen("signals.txt", "w");
    if (fp == NULL)
    {
        printf("Error opening file.\n");
        return 1;
    }
    fprintf(fp, "orign\t in\t out1\t out2\t out3\n");
    for (int i = 0; i < SIGNAL_LENGTH; i++)
    {
        fprintf(fp, "%f\t %f\t %f\t %f\t %f\n", orign[i], input_signal[i], output_signal1[i], output_signal2[i]);
    }

    fclose(fp);

    return 0;
}

#include <windows.h>
// #include <math.h>

#define PI 3.14159265358979323846

// 窗口过程函数
LRESULT CALLBACK WndProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam)
{
    switch (msg)
    {
    case WM_PAINT:
    {
        PAINTSTRUCT ps;
        HDC hdc = BeginPaint(hwnd, &ps);

        // 设置绘图模式为抗锯齿
        SetBkMode(hdc, TRANSPARENT);

        // 定义4种不同颜色的画笔
        HPEN hPens[4];
        hPens[0] = CreatePen(PS_SOLID, 2, RGB(255, 0, 0));   // 红色
        hPens[1] = CreatePen(PS_SOLID, 2, RGB(0, 255, 0));   // 绿色
        hPens[2] = CreatePen(PS_SOLID, 2, RGB(0, 0, 255));   // 蓝色
        hPens[3] = CreatePen(PS_SOLID, 2, RGB(255, 0, 255)); // 黄色

        HBRUSH hBrush = (HBRUSH)GetStockObject(NULL_BRUSH);

        // 绘制坐标轴
        MoveToEx(hdc, 50, HEIGHT / 2, NULL);
        LineTo(hdc, WIDTH * 10 - 50, HEIGHT / 2);
        MoveToEx(hdc, WIDTH * 10 / 2, 50, NULL);
        LineTo(hdc, WIDTH * 10 / 2, HEIGHT - 50);

        // 定义4条曲线的相位
        double phases[4] = {0, 0, 0, 0};
        float *data[4] = {orign, input_signal, output_signal1, output_signal2};
        // 绘制4条曲线
        for (int curveIndex = 0; curveIndex < 4; curveIndex++)
        {
            SelectObject(hdc, hPens[curveIndex]);
            SelectObject(hdc, hBrush);

            for (int x = 5; x < WIDTH + 5; x++)
            {
                // double t = (double)(x - 50) / (WIDTH - 100) * 2 * PI;
                // float *p = data[curveIndex];
                double yValue = data[curveIndex][x - 5];

                // double yValue = sin(t + phases[curveIndex]);

                // double yValue = data[curveIndex][(t + phases[curveIndex]) * 100];
                int y = (int)(HEIGHT / 2 - yValue * (HEIGHT / 4));
                // int y = yValue;
                if (x == 5)
                {
                    MoveToEx(hdc, x, y, NULL);
                    LineTo(hdc, 5 * 10, y);
                }
                else
                {
                    LineTo(hdc, x * 10, y);
                }
            }
        }

        // 删除画笔
        for (int i = 0; i < 4; i++)
        {
            DeleteObject(hPens[i]);
        }
        DeleteObject(hBrush);
        EndPaint(hwnd, &ps);
        break;
    }
    case WM_DESTROY:
        PostQuitMessage(0);
        break;
    default:
        return DefWindowProc(hwnd, msg, wParam, lParam);
    }
    return 0;
}

// 主函数
int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, PSTR szCmdLine, int iCmdShow)
{
    // 注册窗口类
    WNDCLASS wc = {0};
    wc.lpfnWndProc = WndProc;
    wc.hInstance = hInstance;
    wc.lpszClassName = "SineWaveWindow";
    wc.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
    RegisterClass(&wc);

    // 创建窗口
    HWND hwnd = CreateWindow(wc.lpszClassName, "Sine Wave", WS_OVERLAPPEDWINDOW,
                             CW_USEDEFAULT, CW_USEDEFAULT, WIDTH * 10, HEIGHT, NULL, NULL, hInstance, NULL);
    generate_ware();
    // 显示窗口
    ShowWindow(hwnd, iCmdShow);
    UpdateWindow(hwnd);

    // 消息循环
    MSG msg;
    while (GetMessage(&msg, NULL, 0, 0))
    {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }
    return msg.wParam;
}

// #define ARM_MATH_DSP
// void arm_mult_f32(
//     float *pSrcA,
//     float *pSrcB,
//     float *pDst,
//     uint32_t blockSize)
// {
//     uint32_t blkCnt; /* loop counters */
// #if defined(ARM_MATH_DSP)

//     /* Run the below code for Cortex-M4 and Cortex-M3 */
//     float inA1, inA2, inA3, inA4; /* temporary input variables */
//     float inB1, inB2, inB3, inB4; /* temporary input variables */
//     float out1, out2, out3, out4; /* temporary output variables */

//     /* loop Unrolling */
//     blkCnt = blockSize >> 2U;

//     /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
//      ** a second loop below computes the remaining 1 to 3 samples. */
//     while (blkCnt > 0U)
//     {
//         /* C = A * B */
//         /* Multiply the inputs and store the results in output buffer */
//         /* read sample from sourceA */
//         inA1 = *pSrcA;
//         /* read sample from sourceB */
//         inB1 = *pSrcB;
//         /* read sample from sourceA */
//         inA2 = *(pSrcA + 1);
//         /* read sample from sourceB */
//         inB2 = *(pSrcB + 1);

//         /* out = sourceA * sourceB */
//         out1 = inA1 * inB1;

//         /* read sample from sourceA */
//         inA3 = *(pSrcA + 2);
//         /* read sample from sourceB */
//         inB3 = *(pSrcB + 2);

//         /* out = sourceA * sourceB */
//         out2 = inA2 * inB2;

//         /* read sample from sourceA */
//         inA4 = *(pSrcA + 3);

//         /* store result to destination buffer */
//         *pDst = out1;

//         /* read sample from sourceB */
//         inB4 = *(pSrcB + 3);

//         /* out = sourceA * sourceB */
//         out3 = inA3 * inB3;

//         /* store result to destination buffer */
//         *(pDst + 1) = out2;

//         /* out = sourceA * sourceB */
//         out4 = inA4 * inB4;
//         /* store result to destination buffer */
//         *(pDst + 2) = out3;
//         /* store result to destination buffer */
//         *(pDst + 3) = out4;

//         /* update pointers to process next samples */
//         pSrcA += 4U;
//         pSrcB += 4U;
//         pDst += 4U;

//         /* Decrement the blockSize loop counter */
//         blkCnt--;
//     }

//     /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
//      ** No loop unrolling is used. */
//     blkCnt = blockSize % 0x4U;

// #else

//     /* Run the below code for Cortex-M0 */

//     /* Initialize blkCnt with number of samples */
//     blkCnt = blockSize;

// #endif /* #if defined (ARM_MATH_DSP) */

//     while (blkCnt > 0U)
//     {
//         /* C = A * B */
//         /* Multiply the inputs and store the results in output buffer */
//         *pDst++ = (*pSrcA++) * (*pSrcB++);

//         /* Decrement the blockSize loop counter */
//         blkCnt--;
//     }
// }

// void arm_radix8_butterfly_f32(
//     float *pSrc,
//     uint16_t fftLen,
//     const float *pCoef,
//     uint16_t twidCoefModifier)
// {
//     uint32_t ia1, ia2, ia3, ia4, ia5, ia6, ia7;
//     uint32_t i1, i2, i3, i4, i5, i6, i7, i8;
//     uint32_t id;
//     uint32_t n1, n2, j;

//     float r1, r2, r3, r4, r5, r6, r7, r8;
//     float t1, t2;
//     float s1, s2, s3, s4, s5, s6, s7, s8;
//     float p1, p2, p3, p4;
//     float co2, co3, co4, co5, co6, co7, co8;
//     float si2, si3, si4, si5, si6, si7, si8;
//     const float C81 = 0.70710678118f;

//     n2 = fftLen;

//     do
//     {
//         n1 = n2;
//         n2 = n2 >> 3;
//         i1 = 0;

//         do
//         {
//             i2 = i1 + n2;
//             i3 = i2 + n2;
//             i4 = i3 + n2;
//             i5 = i4 + n2;
//             i6 = i5 + n2;
//             i7 = i6 + n2;
//             i8 = i7 + n2;
//             r1 = pSrc[2 * i1] + pSrc[2 * i5];
//             r5 = pSrc[2 * i1] - pSrc[2 * i5];
//             r2 = pSrc[2 * i2] + pSrc[2 * i6];
//             r6 = pSrc[2 * i2] - pSrc[2 * i6];
//             r3 = pSrc[2 * i3] + pSrc[2 * i7];
//             r7 = pSrc[2 * i3] - pSrc[2 * i7];
//             r4 = pSrc[2 * i4] + pSrc[2 * i8];
//             r8 = pSrc[2 * i4] - pSrc[2 * i8];
//             t1 = r1 - r3;
//             r1 = r1 + r3;
//             r3 = r2 - r4;
//             r2 = r2 + r4;
//             pSrc[2 * i1] = r1 + r2;
//             pSrc[2 * i5] = r1 - r2;
//             r1 = pSrc[2 * i1 + 1] + pSrc[2 * i5 + 1];
//             s5 = pSrc[2 * i1 + 1] - pSrc[2 * i5 + 1];
//             r2 = pSrc[2 * i2 + 1] + pSrc[2 * i6 + 1];
//             s6 = pSrc[2 * i2 + 1] - pSrc[2 * i6 + 1];
//             s3 = pSrc[2 * i3 + 1] + pSrc[2 * i7 + 1];
//             s7 = pSrc[2 * i3 + 1] - pSrc[2 * i7 + 1];
//             r4 = pSrc[2 * i4 + 1] + pSrc[2 * i8 + 1];
//             s8 = pSrc[2 * i4 + 1] - pSrc[2 * i8 + 1];
//             t2 = r1 - s3;
//             r1 = r1 + s3;
//             s3 = r2 - r4;
//             r2 = r2 + r4;
//             pSrc[2 * i1 + 1] = r1 + r2;
//             pSrc[2 * i5 + 1] = r1 - r2;
//             pSrc[2 * i3] = t1 + s3;
//             pSrc[2 * i7] = t1 - s3;
//             pSrc[2 * i3 + 1] = t2 - r3;
//             pSrc[2 * i7 + 1] = t2 + r3;
//             r1 = (r6 - r8) * C81;
//             r6 = (r6 + r8) * C81;
//             r2 = (s6 - s8) * C81;
//             s6 = (s6 + s8) * C81;
//             t1 = r5 - r1;
//             r5 = r5 + r1;
//             r8 = r7 - r6;
//             r7 = r7 + r6;
//             t2 = s5 - r2;
//             s5 = s5 + r2;
//             s8 = s7 - s6;
//             s7 = s7 + s6;
//             pSrc[2 * i2] = r5 + s7;
//             pSrc[2 * i8] = r5 - s7;
//             pSrc[2 * i6] = t1 + s8;
//             pSrc[2 * i4] = t1 - s8;
//             pSrc[2 * i2 + 1] = s5 - r7;
//             pSrc[2 * i8 + 1] = s5 + r7;
//             pSrc[2 * i6 + 1] = t2 - r8;
//             pSrc[2 * i4 + 1] = t2 + r8;

//             i1 += n1;
//         } while (i1 < fftLen);

//         if (n2 < 8)
//             break;

//         ia1 = 0;
//         j = 1;

//         do
//         {
//             /*  index calculation for the coefficients */
//             id = ia1 + twidCoefModifier;
//             ia1 = id;
//             ia2 = ia1 + id;
//             ia3 = ia2 + id;
//             ia4 = ia3 + id;
//             ia5 = ia4 + id;
//             ia6 = ia5 + id;
//             ia7 = ia6 + id;

//             co2 = pCoef[2 * ia1];
//             co3 = pCoef[2 * ia2];
//             co4 = pCoef[2 * ia3];
//             co5 = pCoef[2 * ia4];
//             co6 = pCoef[2 * ia5];
//             co7 = pCoef[2 * ia6];
//             co8 = pCoef[2 * ia7];
//             si2 = pCoef[2 * ia1 + 1];
//             si3 = pCoef[2 * ia2 + 1];
//             si4 = pCoef[2 * ia3 + 1];
//             si5 = pCoef[2 * ia4 + 1];
//             si6 = pCoef[2 * ia5 + 1];
//             si7 = pCoef[2 * ia6 + 1];
//             si8 = pCoef[2 * ia7 + 1];

//             i1 = j;

//             do
//             {
//                 /*  index calculation for the input */
//                 i2 = i1 + n2;
//                 i3 = i2 + n2;
//                 i4 = i3 + n2;
//                 i5 = i4 + n2;
//                 i6 = i5 + n2;
//                 i7 = i6 + n2;
//                 i8 = i7 + n2;
//                 r1 = pSrc[2 * i1] + pSrc[2 * i5];
//                 r5 = pSrc[2 * i1] - pSrc[2 * i5];
//                 r2 = pSrc[2 * i2] + pSrc[2 * i6];
//                 r6 = pSrc[2 * i2] - pSrc[2 * i6];
//                 r3 = pSrc[2 * i3] + pSrc[2 * i7];
//                 r7 = pSrc[2 * i3] - pSrc[2 * i7];
//                 r4 = pSrc[2 * i4] + pSrc[2 * i8];
//                 r8 = pSrc[2 * i4] - pSrc[2 * i8];
//                 t1 = r1 - r3;
//                 r1 = r1 + r3;
//                 r3 = r2 - r4;
//                 r2 = r2 + r4;
//                 pSrc[2 * i1] = r1 + r2;
//                 r2 = r1 - r2;
//                 s1 = pSrc[2 * i1 + 1] + pSrc[2 * i5 + 1];
//                 s5 = pSrc[2 * i1 + 1] - pSrc[2 * i5 + 1];
//                 s2 = pSrc[2 * i2 + 1] + pSrc[2 * i6 + 1];
//                 s6 = pSrc[2 * i2 + 1] - pSrc[2 * i6 + 1];
//                 s3 = pSrc[2 * i3 + 1] + pSrc[2 * i7 + 1];
//                 s7 = pSrc[2 * i3 + 1] - pSrc[2 * i7 + 1];
//                 s4 = pSrc[2 * i4 + 1] + pSrc[2 * i8 + 1];
//                 s8 = pSrc[2 * i4 + 1] - pSrc[2 * i8 + 1];
//                 t2 = s1 - s3;
//                 s1 = s1 + s3;
//                 s3 = s2 - s4;
//                 s2 = s2 + s4;
//                 r1 = t1 + s3;
//                 t1 = t1 - s3;
//                 pSrc[2 * i1 + 1] = s1 + s2;
//                 s2 = s1 - s2;
//                 s1 = t2 - r3;
//                 t2 = t2 + r3;
//                 p1 = co5 * r2;
//                 p2 = si5 * s2;
//                 p3 = co5 * s2;
//                 p4 = si5 * r2;
//                 pSrc[2 * i5] = p1 + p2;
//                 pSrc[2 * i5 + 1] = p3 - p4;
//                 p1 = co3 * r1;
//                 p2 = si3 * s1;
//                 p3 = co3 * s1;
//                 p4 = si3 * r1;
//                 pSrc[2 * i3] = p1 + p2;
//                 pSrc[2 * i3 + 1] = p3 - p4;
//                 p1 = co7 * t1;
//                 p2 = si7 * t2;
//                 p3 = co7 * t2;
//                 p4 = si7 * t1;
//                 pSrc[2 * i7] = p1 + p2;
//                 pSrc[2 * i7 + 1] = p3 - p4;
//                 r1 = (r6 - r8) * C81;
//                 r6 = (r6 + r8) * C81;
//                 s1 = (s6 - s8) * C81;
//                 s6 = (s6 + s8) * C81;
//                 t1 = r5 - r1;
//                 r5 = r5 + r1;
//                 r8 = r7 - r6;
//                 r7 = r7 + r6;
//                 t2 = s5 - s1;
//                 s5 = s5 + s1;
//                 s8 = s7 - s6;
//                 s7 = s7 + s6;
//                 r1 = r5 + s7;
//                 r5 = r5 - s7;
//                 r6 = t1 + s8;
//                 t1 = t1 - s8;
//                 s1 = s5 - r7;
//                 s5 = s5 + r7;
//                 s6 = t2 - r8;
//                 t2 = t2 + r8;
//                 p1 = co2 * r1;
//                 p2 = si2 * s1;
//                 p3 = co2 * s1;
//                 p4 = si2 * r1;
//                 pSrc[2 * i2] = p1 + p2;
//                 pSrc[2 * i2 + 1] = p3 - p4;
//                 p1 = co8 * r5;
//                 p2 = si8 * s5;
//                 p3 = co8 * s5;
//                 p4 = si8 * r5;
//                 pSrc[2 * i8] = p1 + p2;
//                 pSrc[2 * i8 + 1] = p3 - p4;
//                 p1 = co6 * r6;
//                 p2 = si6 * s6;
//                 p3 = co6 * s6;
//                 p4 = si6 * r6;
//                 pSrc[2 * i6] = p1 + p2;
//                 pSrc[2 * i6 + 1] = p3 - p4;
//                 p1 = co4 * t1;
//                 p2 = si4 * t2;
//                 p3 = co4 * t2;
//                 p4 = si4 * t1;
//                 pSrc[2 * i4] = p1 + p2;
//                 pSrc[2 * i4 + 1] = p3 - p4;

//                 i1 += n1;
//             } while (i1 < fftLen);

//             j++;
//         } while (j < n2);

//         twidCoefModifier <<= 3;
//     } while (n2 > 7);
// }
// void arm_radix8_butterfly_f32(
//     float *pSrc,
//     uint16_t fftLen,
//     const float *pCoef,
//     uint16_t twidCoefModifier)
// {
//     uint32_t ia1, ia2, ia3, ia4, ia5, ia6, ia7;
//     uint32_t i1, i2, i3, i4, i5, i6, i7, i8;
//     uint32_t id;
//     uint32_t n1, n2, j;

//     float r1, r2, r3, r4, r5, r6, r7, r8;
//     float t1, t2;
//     float s1, s2, s3, s4, s5, s6, s7, s8;
//     float p1, p2, p3, p4;
//     float co2, co3, co4, co5, co6, co7, co8;
//     float si2, si3, si4, si5, si6, si7, si8;
//     const float C81 = 0.70710678118f;

//     n2 = fftLen;

//     do
//     {
//         n1 = n2;
//         n2 = n2 >> 3;
//         i1 = 0;

//         do
//         {
//             i2 = i1 + n2;
//             i3 = i2 + n2;
//             i4 = i3 + n2;
//             i5 = i4 + n2;
//             i6 = i5 + n2;
//             i7 = i6 + n2;
//             i8 = i7 + n2;
//             r1 = pSrc[2 * i1] + pSrc[2 * i5];
//             r5 = pSrc[2 * i1] - pSrc[2 * i5];
//             r2 = pSrc[2 * i2] + pSrc[2 * i6];
//             r6 = pSrc[2 * i2] - pSrc[2 * i6];
//             r3 = pSrc[2 * i3] + pSrc[2 * i7];
//             r7 = pSrc[2 * i3] - pSrc[2 * i7];
//             r4 = pSrc[2 * i4] + pSrc[2 * i8];
//             r8 = pSrc[2 * i4] - pSrc[2 * i8];
//             t1 = r1 - r3;
//             r1 = r1 + r3;
//             r3 = r2 - r4;
//             r2 = r2 + r4;
//             pSrc[2 * i1] = r1 + r2;
//             pSrc[2 * i5] = r1 - r2;
//             r1 = pSrc[2 * i1 + 1] + pSrc[2 * i5 + 1];
//             s5 = pSrc[2 * i1 + 1] - pSrc[2 * i5 + 1];
//             r2 = pSrc[2 * i2 + 1] + pSrc[2 * i6 + 1];
//             s6 = pSrc[2 * i2 + 1] - pSrc[2 * i6 + 1];
//             s3 = pSrc[2 * i3 + 1] + pSrc[2 * i7 + 1];
//             s7 = pSrc[2 * i3 + 1] - pSrc[2 * i7 + 1];
//             r4 = pSrc[2 * i4 + 1] + pSrc[2 * i8 + 1];
//             s8 = pSrc[2 * i4 + 1] - pSrc[2 * i8 + 1];
//             t2 = r1 - s3;
//             r1 = r1 + s3;
//             s3 = r2 - r4;
//             r2 = r2 + r4;
//             pSrc[2 * i1 + 1] = r1 + r2;
//             pSrc[2 * i5 + 1] = r1 - r2;
//             pSrc[2 * i3] = t1 + s3;
//             pSrc[2 * i7] = t1 - s3;
//             pSrc[2 * i3 + 1] = t2 - r3;
//             pSrc[2 * i7 + 1] = t2 + r3;
//             r1 = (r6 - r8) * C81;
//             r6 = (r6 + r8) * C81;
//             r2 = (s6 - s8) * C81;
//             s6 = (s6 + s8) * C81;
//             t1 = r5 - r1;
//             r5 = r5 + r1;
//             r8 = r7 - r6;
//             r7 = r7 + r6;
//             t2 = s5 - r2;
//             s5 = s5 + r2;
//             s8 = s7 - s6;
//             s7 = s7 + s6;
//             pSrc[2 * i2] = r5 + s7;
//             pSrc[2 * i8] = r5 - s7;
//             pSrc[2 * i6] = t1 + s8;
//             pSrc[2 * i4] = t1 - s8;
//             pSrc[2 * i2 + 1] = s5 - r7;
//             pSrc[2 * i8 + 1] = s5 + r7;
//             pSrc[2 * i6 + 1] = t2 - r8;
//             pSrc[2 * i4 + 1] = t2 + r8;

//             i1 += n1;
//         } while (i1 < fftLen);

//         if (n2 < 8)
//             break;

//         ia1 = 0;
//         j = 1;

//         do
//         {
//             /*  index calculation for the coefficients */
//             id = ia1 + twidCoefModifier;
//             ia1 = id;
//             ia2 = ia1 + id;
//             ia3 = ia2 + id;
//             ia4 = ia3 + id;
//             ia5 = ia4 + id;
//             ia6 = ia5 + id;
//             ia7 = ia6 + id;

//             co2 = pCoef[2 * ia1];
//             co3 = pCoef[2 * ia2];
//             co4 = pCoef[2 * ia3];
//             co5 = pCoef[2 * ia4];
//             co6 = pCoef[2 * ia5];
//             co7 = pCoef[2 * ia6];
//             co8 = pCoef[2 * ia7];
//             si2 = pCoef[2 * ia1 + 1];
//             si3 = pCoef[2 * ia2 + 1];
//             si4 = pCoef[2 * ia3 + 1];
//             si5 = pCoef[2 * ia4 + 1];
//             si6 = pCoef[2 * ia5 + 1];
//             si7 = pCoef[2 * ia6 + 1];
//             si8 = pCoef[2 * ia7 + 1];

//             i1 = j;

//             do
//             {
//                 /*  index calculation for the input */
//                 i2 = i1 + n2;
//                 i3 = i2 + n2;
//                 i4 = i3 + n2;
//                 i5 = i4 + n2;
//                 i6 = i5 + n2;
//                 i7 = i6 + n2;
//                 i8 = i7 + n2;
//                 r1 = pSrc[2 * i1] + pSrc[2 * i5];
//                 r5 = pSrc[2 * i1] - pSrc[2 * i5];
//                 r2 = pSrc[2 * i2] + pSrc[2 * i6];
//                 r6 = pSrc[2 * i2] - pSrc[2 * i6];
//                 r3 = pSrc[2 * i3] + pSrc[2 * i7];
//                 r7 = pSrc[2 * i3] - pSrc[2 * i7];
//                 r4 = pSrc[2 * i4] + pSrc[2 * i8];
//                 r8 = pSrc[2 * i4] - pSrc[2 * i8];
//                 t1 = r1 - r3;
//                 r1 = r1 + r3;
//                 r3 = r2 - r4;
//                 r2 = r2 + r4;
//                 pSrc[2 * i1] = r1 + r2;
//                 r2 = r1 - r2;
//                 s1 = pSrc[2 * i1 + 1] + pSrc[2 * i5 + 1];
//                 s5 = pSrc[2 * i1 + 1] - pSrc[2 * i5 + 1];
//                 s2 = pSrc[2 * i2 + 1] + pSrc[2 * i6 + 1];
//                 s6 = pSrc[2 * i2 + 1] - pSrc[2 * i6 + 1];
//                 s3 = pSrc[2 * i3 + 1] + pSrc[2 * i7 + 1];
//                 s7 = pSrc[2 * i3 + 1] - pSrc[2 * i7 + 1];
//                 s4 = pSrc[2 * i4 + 1] + pSrc[2 * i8 + 1];
//                 s8 = pSrc[2 * i4 + 1] - pSrc[2 * i8 + 1];
//                 t2 = s1 - s3;
//                 s1 = s1 + s3;
//                 s3 = s2 - s4;
//                 s2 = s2 + s4;
//                 r1 = t1 + s3;
//                 t1 = t1 - s3;
//                 pSrc[2 * i1 + 1] = s1 + s2;
//                 s2 = s1 - s2;
//                 s1 = t2 - r3;
//                 t2 = t2 + r3;
//                 p1 = co5 * r2;
//                 p2 = si5 * s2;
//                 p3 = co5 * s2;
//                 p4 = si5 * r2;
//                 pSrc[2 * i5] = p1 + p2;
//                 pSrc[2 * i5 + 1] = p3 - p4;
//                 p1 = co3 * r1;
//                 p2 = si3 * s1;
//                 p3 = co3 * s1;
//                 p4 = si3 * r1;
//                 pSrc[2 * i3] = p1 + p2;
//                 pSrc[2 * i3 + 1] = p3 - p4;
//                 p1 = co7 * t1;
//                 p2 = si7 * t2;
//                 p3 = co7 * t2;
//                 p4 = si7 * t1;
//                 pSrc[2 * i7] = p1 + p2;
//                 pSrc[2 * i7 + 1] = p3 - p4;
//                 r1 = (r6 - r8) * C81;
//                 r6 = (r6 + r8) * C81;
//                 s1 = (s6 - s8) * C81;
//                 s6 = (s6 + s8) * C81;
//                 t1 = r5 - r1;
//                 r5 = r5 + r1;
//                 r8 = r7 - r6;
//                 r7 = r7 + r6;
//                 t2 = s5 - s1;
//                 s5 = s5 + s1;
//                 s8 = s7 - s6;
//                 s7 = s7 + s6;
//                 r1 = r5 + s7;
//                 r5 = r5 - s7;
//                 r6 = t1 + s8;
//                 t1 = t1 - s8;
//                 s1 = s5 - r7;
//                 s5 = s5 + r7;
//                 s6 = t2 - r8;
//                 t2 = t2 + r8;
//                 p1 = co2 * r1;
//                 p2 = si2 * s1;
//                 p3 = co2 * s1;
//                 p4 = si2 * r1;
//                 pSrc[2 * i2] = p1 + p2;
//                 pSrc[2 * i2 + 1] = p3 - p4;
//                 p1 = co8 * r5;
//                 p2 = si8 * s5;
//                 p3 = co8 * s5;
//                 p4 = si8 * r5;
//                 pSrc[2 * i8] = p1 + p2;
//                 pSrc[2 * i8 + 1] = p3 - p4;
//                 p1 = co6 * r6;
//                 p2 = si6 * s6;
//                 p3 = co6 * s6;
//                 p4 = si6 * r6;
//                 pSrc[2 * i6] = p1 + p2;
//                 pSrc[2 * i6 + 1] = p3 - p4;
//                 p1 = co4 * t1;
//                 p2 = si4 * t2;
//                 p3 = co4 * t2;
//                 p4 = si4 * t1;
//                 pSrc[2 * i4] = p1 + p2;
//                 pSrc[2 * i4 + 1] = p3 - p4;

//                 i1 += n1;
//             } while (i1 < fftLen);

//             j++;
//         } while (j < n2);

//         twidCoefModifier <<= 3;
//     } while (n2 > 7);
// }
// typedef enum
// {
//   ARM_MATH_SUCCESS = 0,                /**< No error */
//   ARM_MATH_ARGUMENT_ERROR = -1,        /**< One or more arguments are incorrect */
//   ARM_MATH_LENGTH_ERROR = -2,          /**< Length of data buffer is incorrect */
//   ARM_MATH_SIZE_MISMATCH = -3,         /**< Size of matrices is not compatible with the operation. */
//   ARM_MATH_NANINF = -4,                /**< Not-a-number (NaN) or infinity is generated */
//   ARM_MATH_SINGULAR = -5,              /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
//   ARM_MATH_TEST_FAILURE = -6           /**< Test Failed  */
// } arm_status;
// typedef struct
// {
//   uint16_t fftLen;                   /**< length of the FFT. */
//   const float *pTwiddle;         /**< points to the Twiddle factor table. */
//   const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */
//   uint16_t bitRevLength;             /**< bit reversal table length. */
// } arm_cfft_instance_f32;
// typedef struct
//   {
//     arm_cfft_instance_f32 Sint;      /**< Internal CFFT structure. */
//     uint16_t fftLenRFFT;             /**< length of the real sequence */
//     float * pTwiddleRFFT;        /**< Twiddle factors real stage  */
//   } arm_rfft_fast_instance_f32 ;
// arm_status arm_rfft_fast_init_f32(
//     arm_rfft_fast_instance_f32 * S,
//     uint16_t fftLen)
//   {
//     arm_cfft_instance_f32 * Sint;
//     /*  Initialise the default arm status */
//     arm_status status = ARM_MATH_SUCCESS;
//     /*  Initialise the FFT length */
//     Sint = &(S->Sint);
//     Sint->fftLen = fftLen/2;
//     S->fftLenRFFT = fftLen;

//     /*  Initializations of structure parameters depending on the FFT length */
//     switch (Sint->fftLen)
//     {
//     case 2048U:
//       /*  Initializations of structure parameters for 2048 point FFT */
//       /*  Initialise the bit reversal table length */
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_2048_TABLE_LENGTH;
//       /*  Initialise the bit reversal table pointer */
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable2048;
//       /*  Initialise the Twiddle coefficient pointers */
//           Sint->pTwiddle     = (float *) twiddleCoef_2048;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_4096;
//       break;
//     case 1024U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_1024_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable1024;
//           Sint->pTwiddle     = (float *) twiddleCoef_1024;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_2048;
//       break;
//     case 512U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_512_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable512;
//           Sint->pTwiddle     = (float *) twiddleCoef_512;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_1024;
//       break;
//     case 256U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_256_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable256;
//           Sint->pTwiddle     = (float *) twiddleCoef_256;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_512;
//       break;
//     case 128U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_128_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable128;
//           Sint->pTwiddle     = (float *) twiddleCoef_128;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_256;
//       break;
//     case 64U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_64_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable64;
//           Sint->pTwiddle     = (float *) twiddleCoef_64;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_128;
//       break;
//     case 32U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_32_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable32;
//           Sint->pTwiddle     = (float *) twiddleCoef_32;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_64;
//       break;
//     case 16U:
//       Sint->bitRevLength = ARMBITREVINDEXTABLE_16_TABLE_LENGTH;
//       Sint->pBitRevTable = (uint16_t *)armBitRevIndexTable16;
//           Sint->pTwiddle     = (float *) twiddleCoef_16;
//           S->pTwiddleRFFT    = (float *) twiddleCoef_rfft_32;
//       break;
//     default:
//       /*  Reporting argument error if fftSize is not valid value */
//       status = ARM_MATH_ARGUMENT_ERROR;
//       break;
//     }

//     return (status);
//   }

/**
 * @} end of RealFFT group
 */

// void fft_test()
// {
//     float fftData[FFT_WINDOW_SIZE];

//     float hanningWindow[FFT_WINDOW_SIZE];
//     arm_rfft_fast_instance_f32 fftInstance;
//     arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
//     #define FFT_BIN_COUNT             (FFT_WINDOW_SIZE / 2) // 16
//     for (int circularBufferIdx = 0; circularBufferIdx < FFT_WINDOW_SIZE; circularBufferIdx++)
//     {
//         const uint8_t ringBufIdx = FFT_WINDOW_SIZE - circularBufferIdx;
//         arm_mult_f32(&input_signal[circularBufferIdx], &hanningWindow[0], &fftData[0], ringBufIdx);
//         if (circularBufferIdx > 0)
//         {
//             arm_mult_f32(&input_signal[0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], circularBufferIdx);
//         }
//     }
//     arm_radix8_butterfly_f32(fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
// }
